#include "btl_config.h"
#include "fp_algorithm.h"
#include "fp_util.h"
#include "fp_timer.h"
#include "fp_led.h"

#define SCHED_TEST 0

void sched_fingerprint(void);
void bfxx_fingerprint_task(void);
void bfxx_protocol_task(void);
void bfxx_test_task(void);
int isTestEn(void);

void sched_init(void)
{
    printf("sched_init");
    // os_init();

    fp_led_init();
    fp_moto_init();
    fp_timer_init();
    fp_interrupt_and_reset_init();
    fp_spi_init();
#ifdef __BTL_FP_ALGORITHM__
    fp_algorithm_init();
#endif /* __BTL_FP_ALGORITHM__ */
    bf_print_d("sys clock: %u", clk_get("sys"));

    mem_stats();
#if SCHED_TEST
    os_task_create(sched_fingerprint, NULL, 32, 4 * 1024, 1 * 1024, "fp_task");
#endif
}

void sched_fingerprint(void)
{
    // bf_print_d("enter");
    // bf_print_d("sys clock: %u", clk_get("sys"));
    // sys_timer_add(NULL, timer_handle, TIMEROUT);
    /* Main loop */
#if SCHED_TEST
    while (1) {
#else
    int cnt = 100;
    while (--cnt) {
#endif
        clr_wdt();
        /* Low power manager unit */
        // bfxx_power_task();
#ifdef __BTL_FP_TEST__
        bfxx_test_task();
        if (isTestEn())
            continue;
#endif /* __BTL_FP_TEST__ */

        clr_wdt();
        /* Check if there is any command from host */
        bfxx_protocol_task();

#ifdef __BTL_FP_ALGORITHM__
        /* Process transaction that waiting forever until finger down, such as:
         * 1. do_capture()
         * 2. do_verify()
         * 3. do_enroll()
         */
        bfxx_fingerprint_task();
#endif /* __BTL_FP_ALGORITHM__ */
    }

    /* Exit */
}

static int task_msg_handle(CASE_TASK_MSG msg, u8* buf, u8 len)
{
    // bf_print_d("%s[msg:%d len:%d]", __func__, msg, len);
    int ret = 0;
    switch (msg) {
        case CASE_TASK_MSG_0:
            // bf_print_d("CASE_TASK_MSG_0");
            sched_fingerprint();
            break;
        // case CASE_TASK_MSG_1:
        //     log_info("CASE_TASK_MSG_1");
        //     log_info_hexdump(buf, len);
        //     break;
        default:
            break;
    }
    return ret;
}


void work_handle(void);
static int work_task_msg_handle(CASE_TASK_MSG msg, u8* buf, u8 len)
{
    // bf_print_d("%s[msg:%d len:%d]", __func__, msg, len);
    int ret = 0;
    switch (msg) {
        case CASE_TASK_MSG_0:
            // bf_print_d("CASE_TASK_MSG_0");
            // sched_fingerprint();
            // work_handle();
            break;
        // case CASE_TASK_MSG_1:
        //     log_info("CASE_TASK_MSG_1");
        //     log_info_hexdump(buf, len);
        //     break;
        default:
            break;
    }
    return ret;
}

static int task_msg_post(CASE_TASK_MSG msg, u8* buf, u8 len)
{
    // bf_print_d("%s[msg:%d len:%d]", __func__, msg, len);
    int ret = OS_NO_ERR;
    if (buf == NULL) {
        ret = os_taskq_post_msg(CASE_TASK_NAME, 1, msg);
    } else {
        if (len > MAX_BUF) {
            ret = -1;
            goto _post_end;
        }
        log_debug_hexdump(buf, len);
        static u8 data[MAX_BUF + 5] ALIGNED(4);
        memset(data, 0x00, sizeof(data));
        data[4] = len;
        memcpy(data + 5, buf, len);
        ((u32*)data)[0] = msg;
        ret = os_taskq_post_type(CASE_TASK_NAME, Q_MSG, MAX_MSG - 1, data);
    }
_post_end:
    // if (ret)
    //     bf_print_d("%s[ret:%d]", __func__, ret);
    return ret;
}

static int work_task_msg_post(CASE_TASK_MSG msg, u8* buf, u8 len)
{
    // bf_print_d("%s[msg:%d len:%d]", __func__, msg, len);
    int ret = OS_NO_ERR;
    if (buf == NULL) {
        ret = os_taskq_post_msg(FP_WORK_NAME, 1, msg);
    } else {
        if (len > MAX_BUF) {
            ret = -1;
            goto _post_end;
        }
        log_debug_hexdump(buf, len);
        static u8 data[MAX_BUF + 5] ALIGNED(4);
        memset(data, 0x00, sizeof(data));
        data[4] = len;
        memcpy(data + 5, buf, len);
        ((u32*)data)[0] = msg;
        ret = os_taskq_post_type(FP_WORK_NAME, Q_MSG, MAX_MSG - 1, data);
    }
_post_end:
    if (ret)
        bf_print_d("%s[ret:%d]", __func__, ret);
    return ret;
}

static void fp_task(void* p)
{
#if (TASK_RUN_WAY == TASK_RUN_MSG)
    int ret = 0;
    u32 msg[MAX_MSG];
    while (1) {
        clr_wdt();
        memset(msg, 0x00, sizeof(msg));
        ret = os_taskq_pend("fp_task", msg, ARRAY_SIZE(msg));
        if (ret != OS_TASKQ) {
            continue;
        }
        if (msg[0] != Q_MSG) {
            continue;
        }
        // bf_print_d("%s[enter_msg:0x%x]", __func__, msg[1]);
        u8* buf = (u8*)(msg + 2);
        ret = task_msg_handle(msg[1], buf + 1, buf[0]);
        // bf_print_d("%s[exit_msg:0x%x -> ret:%d]", __func__, msg[1], ret);
    }
#endif
}

static void fp_work_task(void* p)
{
#if (TASK_RUN_WAY == TASK_RUN_MSG)
    int ret = 0;
    u32 msg[MAX_MSG];
    while (1) {
        clr_wdt();
        memset(msg, 0x00, sizeof(msg));
        ret = os_taskq_pend(FP_WORK_NAME, msg, ARRAY_SIZE(msg));
        if (ret != OS_TASKQ) {
            continue;
        }
        if (msg[0] != Q_MSG) {
            continue;
        }
        // bf_print_d("%s[enter_msg:0x%x]", __func__, msg[1]);
        u8* buf = (u8*)(msg + 2);
        ret = work_task_msg_handle(msg[1], buf + 1, buf[0]);
        // bf_print_d("%s[exit_msg:0x%x -> ret:%d]", __func__, msg[1], ret);
    }
#endif
}

int fp_task_init(void)
{
    bf_print_d("%s[MAX_BUF:%d]", __func__, MAX_BUF);
    int ret = OS_NO_ERR;

    ret = os_task_create(fp_task, NULL, CASE_TASK_PRIO, CASE_TASK_STACK, CASE_TASK_QSIZE,
                         CASE_TASK_NAME);
    if (ret != OS_NO_ERR)
        bf_print_d("%s %s create fail 0x%x", __func__, CASE_TASK_NAME, ret);
    return ret;
}

int fp_work_task_init(void)
{
    bf_print_d("%s[MAX_BUF:%d]", __func__, MAX_BUF);
    int ret = OS_NO_ERR;

    ret = os_task_create(fp_work_task, NULL, 2,  4 * 1024, CASE_TASK_QSIZE, FP_WORK_NAME);
    if (ret != OS_NO_ERR)
        bf_print_d("%s %s create fail 0x%x", __func__, FP_WORK_NAME, ret);
    return ret;
}

int fp_task_delete(void)
{
    int ret = OS_NO_ERR;
    ret = task_kill(CASE_TASK_NAME);
    bf_print_d("%s[task_kill(%s):%d]", __func__, CASE_TASK_NAME, ret);
    return ret;
}

void sched_task_loop(void)
{
#if !SCHED_TEST
#if (TASK_RUN_WAY == TASK_RUN_MSG)
    task_msg_post(CASE_TASK_MSG_0, NULL, 0);
#endif
#endif
}
static void task_loop(void* priv)
{
    // log_info("只发消息---不带参数");
    task_msg_post(CASE_TASK_MSG_0, NULL, 0);
}

void post_work(void)
{
    work_task_msg_post(CASE_TASK_MSG_0, NULL, 0);
}

int start_fp(void)
{
    bf_print_d("%s", __func__);

    sched_init();
#if !SCHED_TEST
    fp_task_init();
    // fp_work_task_init();
    task_msg_post(CASE_TASK_MSG_0, NULL, 0);
    sys_timer_add(NULL, task_loop, 500 * 1);
#endif
    // sys_s_hi_timer_add(NULL, case_task_hi_loop, 1000);
    /* sys_hi_timer_add(NULL, case_task_hi_loop, 4); */
}

